//
// Created by engineai on 2024/07/03.
//

/*!
 * @file CollisionBox.cpp
 * @brief Collision logic for a box
 */

#include "Collision/CollisionBox.h"
#include "Utilities/utilities.h"

/*!
 * check whether the contact happens or not
 * cp_frame let you know which direction is normal (z) and which directions are
 * x and y. The frame is basically contact coordinate w.r.t global.
 */
template <typename T>
bool CollisionBox<T>::ContactDetection(const Vec3<T> &cp_pos, T &penetration,
                                       Mat3<T> &cp_frame)
{
    Vec3<T> center2cp_local = _orientation.transpose() * (cp_pos - _position);

    bool inside_box(true);
    T err[3] = {0};
    for (size_t i(0); i < 3; ++i)
    {
        // Outside to the positive direction
        if (center2cp_local[i] > _size[i] / 2.)
        {
            inside_box = false;
        }
        // Outside to the negative direction
        else if (center2cp_local[i] < -_size[i] / 2.)
        {
            inside_box = false;
        }
        // Distance from surface
        else
        {
            err[i] = _size[i] / 2. - std::abs(center2cp_local[i]);
        }
    }

    if (inside_box)
    {
        if (err[2] < std::min(err[0], err[1]))
        { // Contact on Z surface
            if (center2cp_local[2] > 0.)
            {
                cp_frame = _orientation;
            }
            else
            {
                cp_frame = -_orientation;
            }
            penetration = -err[2];
        }
        else if (err[1] < std::min(err[0], err[2]))
        { // Contact on Y surface
            if (center2cp_local[1] > 0.)
            {
                // X -->X
                cp_frame.template block<3, 1>(0, 0) =
                    _orientation.template block<3, 1>(0, 0);
                // -Z --> Y
                cp_frame.template block<3, 1>(0, 1) =
                    -_orientation.template block<3, 1>(0, 2);
                // Y --> Z
                cp_frame.template block<3, 1>(0, 2) =
                    _orientation.template block<3, 1>(0, 1);
            }
            else
            {
                // X --> X
                cp_frame.template block<3, 1>(0, 0) =
                    _orientation.template block<3, 1>(0, 0);
                // Z --> Y
                cp_frame.template block<3, 1>(0, 1) =
                    _orientation.template block<3, 1>(0, 2);
                // -Y --> Z
                cp_frame.template block<3, 1>(0, 2) =
                    -_orientation.template block<3, 1>(0, 1);
            }
            penetration = -err[1];
        }
        else
        { // contact on X surface
            if (center2cp_local[0] > 0.)
            {
                // X -->X
                cp_frame.template block<3, 1>(0, 0) =
                    _orientation.template block<3, 1>(0, 1);
                // -Z --> Y
                cp_frame.template block<3, 1>(0, 1) =
                    -_orientation.template block<3, 1>(0, 2);
                // Y --> Z
                cp_frame.template block<3, 1>(0, 2) =
                    _orientation.template block<3, 1>(0, 0);
            }
            else
            {
                // X -->X
                cp_frame.template block<3, 1>(0, 0) =
                    _orientation.template block<3, 1>(0, 0);
                // -Z --> Y
                cp_frame.template block<3, 1>(0, 1) =
                    _orientation.template block<3, 1>(0, 1);
                // Y --> Z
                cp_frame.template block<3, 1>(0, 2) =
                    -_orientation.template block<3, 1>(0, 0);
            }
            penetration = -err[0];
        }
        return true;
    }
    return false;
}

template class CollisionBox<double>;
template class CollisionBox<float>;
